% stickfigure


clf
a=2200;
b=3400;
x_FP=NaN(b,2);
y_FP=NaN(b,2);
for idx=a:b
    l=cos(obj.Kinematics.Beta(idx,:));
    x_FP(idx,:)=obj.Kinematics.position(idx,1)+l.*cos(obj.Kinematics.legAngles(idx,:));
    y_FP(idx,:)=obj.Kinematics.position(idx,3)-l.*sin(obj.Kinematics.legAngles(idx,:));
    if mod(idx,60)==0
        [x,y]=animate_ATRIAS_leg(obj.Kinematics.position(idx,1),obj.Kinematics.position(idx,3),x_FP(idx,1),y_FP(idx,1),0.85);
        plot(x(1:3),y(1:3),'Color',[.8 .8 .8],'Linewidth',2)
        hold on
        [x,y]=animate_ATRIAS_leg(obj.Kinematics.position(idx,1),obj.Kinematics.position(idx,3),x_FP(idx,2),y_FP(idx,2),0.85);
        plot(x(1:3),y(1:3),'Color',[0 0 0],'Linewidth',2)
        axis equal
    end
end
plot(obj.Kinematics.position(a:b,1),obj.Kinematics.position(a:b,3),'Color',[.2 .2 .2],'Linestyle','--','Linewidth',2)
%plot(x_FP(~isnan(x_FP(:,1)),1),y_FP(~isnan(y_FP(:,1)),1))
plot(x_FP(~isnan(x_FP(:,1)),2),y_FP(~isnan(y_FP(:,1)),2),'Color',[.2 .2 .2],'Linestyle','--','Linewidth',2)
set(gca,'yTickLabel',[],'Fontsize',22,'xTickLabel',[0 .2 .4 .6 .8 1 1.2])
c=get(gca,'xlim');
%plot([c(1) c(2)],[-.03 -0.03],'k','Linewidth',10)
ylim([-.1 .9])

